#include "qmi8658a.h"
#include <cstring>
#include <cmath>
#include <freeRTOS/FreeRTOS.h>
#include <esp_log.h>

#define TAG "QMI8658A"

void QMI8658A::init(i2c_master_bus_handle_t i2c_bus, uint8_t addr)
{
    this->m_i2c_addr = addr;
    i2c_device_config_t i2c_config;
    i2c_config.dev_addr_length = I2C_ADDR_BIT_LEN_7;
    i2c_config.device_address = this->m_i2c_addr;
    i2c_config.scl_speed_hz = 200000;
    i2c_config.scl_wait_us = 1000;

    if (this->m_i2c_dev == NULL) {
        i2c_master_bus_add_device(i2c_bus, &i2c_config, &this->m_i2c_dev);
    }

    if (!this->m_i2c_dev) {
        ESP_LOGE(TAG, "Failed to add I2C device");
        return;
    }

    uint8_t data = 0xB0;
    if (write_regs(RESET, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to reset device");
        return;
    }
    vTaskDelay(100 / portTICK_PERIOD_MS);
    // 读取dQY_L寄存器，检查复位是否成功
    if (read_regs(dQY_L, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to read dQY_L");
        return;
    }
    if ((data & 0x80) != 0x80) {
        ESP_LOGE(TAG, "Reset failed");
        return;
    }
    // 配置传感器
    data = 0x60;
    if (write_regs(CTRL1, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL1");
        return;
    }

    // 加速度自检
    if (self_test_acc() != 0) {
        ESP_LOGE(TAG, "Accelerometer self-test failed");
        return;
    }
    // 陀螺仪自检
    if (self_test_gyro() != 0) {
        ESP_LOGE(TAG, "Gyroscope self-test failed");
        return;
    }
    // 配置加速度计
    // 向寄存器CTRL2写入0x33，禁用加速度自检，设置量程为16g，设置输出数据速率为896.8Hz
    data = 0x33;
    if (write_regs(CTRL2, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL2");
        return;
    }
    // 配置陀螺仪
    // 向寄存器CTRL5写入0x35，配置低通滤波器为3.63%
    data = 0x35;
    if (write_regs(CTRL5, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL5");
        return;
    }

    // 设用锁定机制
    // 禁止内部AHB时钟
    data = 0x01;
    if (write_regs(CAL1_L, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CAL1_L");
        return;
    }

    // 在CTRL9协议中写入0x12
    data = 0x12;
    if (write_regs(CTRL9, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL9");
        return;
    }

    vTaskDelay(pdMS_TO_TICKS(100));
    // 等待STATUSINT第七位为1
    data = 0;
    while ((data & 0x80) != 0x80) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return;
        }
    }

    // 向CTRL9寄存器写入CTRL_CMA_ACK(0X00)来确认
    data = 0x00;
    if (write_regs(CTRL9, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL9");
        return;
    }

    // 等待STATUSINT第七位为0
    data = 0xFF;
    while ((data & 0x80) == 0x80) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return;
        }
    }

    // 开启加速度计和陀螺仪，同步采样模式
    // 向寄存器CTRL7写入0x83，启用加速度计和陀螺仪的同步采样模式
    data = 0x83;
    if (write_regs(CTRL7, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL7");
        return;
    }
    // 延时十毫秒，等待模式开启完成
    vTaskDelay(pdMS_TO_TICKS(10));

    // 陀螺仪自带校准
    if(0 != cyr_cod()) {
        ESP_LOGE(TAG, "Failed to perform COD calibration");
        return;
    }

    // 陀螺仪手动校准
    // 循环调用calibration_acc_gry函数，直到校准成功
    while (0 != calibration_acc_gry(this->gyro_compensate)) {
        ESP_LOGE(TAG, "Failed to calibrate gyroscope");
        vTaskDelay(pdMS_TO_TICKS(10));
    }

    // 初始化完成
    ESP_LOGI(TAG, "QMI8658A initialized successfully");

}

void QMI8658A::deinit()
{
    i2c_master_bus_rm_device(this->m_i2c_dev);
    this->m_i2c_dev = NULL;
}


int QMI8658A::self_test_acc()
{
    uint8_t data = 0;
    // 禁用传感器
    // 向寄存器CTRL7写入0x00，禁用加速度计和陀螺仪
    if (write_regs(CTRL7, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL7");
        return -1;
    }

    // 设置合适的加速度计输出速率4G,896.8Hz
    // 向寄存器CTRL2写入0x93以设置加速度计输出数据速率为4G, 896.8Hz
    data = 0x93;
    if (write_regs(CTRL2, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL2");
        return -1;
    }

    // 等待STATUSINT第0位为1
    data = 0;
    while ((data & 0x01) == 0) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }

    // 将CTRL2.aST(第7位设置为0)
    // 读取CTRL2寄存器的值到data
    if (read_regs(CTRL2, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to read CTRL2");
        return -1;
    }

    // 将data的第7位清0
    data &= 0x7F;

    if (write_regs(CTRL2, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL2");
        return -1;
    }

    // 等待STATUSINT第0位为0
    data = 0xFF;
    while ((data & 0x01) == 1) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }

    // 读取加速计自检结果
    uint8_t datas[6] = {0};
    if (read_regs(0x51, datas, 6) != 0) {
        ESP_LOGE(TAG, "Failed to read STATUSINT");
        return -1;
    }

    // 判断结果绝对值是否高于200mg
    int16_t dVxData = 0, dVyData = 0, dVzData = 0;
    dVxData = (int16_t)((datas[1] << 8) | datas[0]);
    dVyData = (int16_t)((datas[3] << 8) | datas[2]);
    dVzData = (int16_t)((datas[5] << 8) | datas[4]);

    // 判断自检结果是否高于200mg
    if (fabs(dVxData * 0.5) < 200 || fabs(dVyData * 0.5) < 200 || fabs(dVzData * 0.5) < 200) {
        ESP_LOGE(TAG, "Accelerometer self-test failed");
        return -1;
    }

    ESP_LOGI(TAG, "Accelerometer self-test passed");
    return 0;

}

int QMI8658A::self_test_gyro()
{
    uint8_t data = 0;
    // 禁用传感器
    // 向寄存器CTRL7写入0x00，禁用加速度计和陀螺仪
    if (write_regs(CTRL7, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL7");
        return -1;
    }

    // 将gST位设置为1(CTRL第7位)
    // 读取寄存器CTRL3的值到data
    if (read_regs(CTRL3, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to read CTRL3");
        return -1;
    }
    // 将data的第7位置1
    data |= 0x80;
    if (write_regs(CTRL3, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL3");
        return -1;
    }
    // 等待STATUSINT第0位为1
    data = 0;
    while ((data & 0x01) == 0) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }
    // 将CTRL3.gST(第7位设置为0)
    // 读取CTRL3寄存器的值到data
    if (read_regs(CTRL3, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to read CTRL3");
        return -1;
    }
    // 将data的第7位清0
    data &= 0x7F;
    if (write_regs(CTRL3, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL3");
        return -1;
    }
    // 等待STATUSINT第0位为0
    data = 0xFF;
    while ((data & 0x01) == 1) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }
    // 读取陀螺仪自检结果
    uint8_t datas[6] = {0};
    if (read_regs(0x51, datas, 6) != 0) {
        ESP_LOGE(TAG, "Failed to read STATUSINT");
        return -1;
    }
    // 判断结果绝对值是否高于300dps
    int16_t dVxData = 0, dVyData = 0, dVzData = 0;
    dVxData = (int16_t)((datas[1] << 8) | datas[0]);
    dVyData = (int16_t)((datas[3] << 8) | datas[2]);
    dVzData = (int16_t)((datas[5] << 8) | datas[4]);
    // 判断自检结果是否高于300dps
    if( fabs(dVxData * 62.5 / 1000.0f) < 300 || fabs(dVyData * 62.5 / 1000.0f) < 300 || fabs(dVzData * 62.5 / 1000.0f) < 300) {
        ESP_LOGE(TAG, "Gyroscope self-test failed");
        return -1;
    }
    ESP_LOGI(TAG, "Gyroscope self-test passed");
    return 0;
}

int QMI8658A::read_raw_data(int16_t *raw_data)
{
    uint8_t data = 0;
    // 判断数据是否有效
    while ((data & 0x01) != 1) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }

    // 判断数据是否锁定
    data = 0;
    while (((data >> 1) & 0x01) != 1) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }

    // 读取传感器数据寄存器
    uint8_t datas[12] = {0};
    if (read_regs(A_XYZ, datas, 12) != 0) {
        ESP_LOGE(TAG, "Failed to read sensor data");
        return -1;
    }

    // 组合数据
    raw_data[0] = (int16_t)((datas[1] << 8) | datas[0]);    // AX
    raw_data[1] = (int16_t)((datas[3] << 8) | datas[2]);    // AY
    raw_data[2] = (int16_t)((datas[5] << 8) | datas[4]);    // AZ
    raw_data[3] = (int16_t)((datas[7] << 8) | datas[6]);    // GX
    raw_data[4] = (int16_t)((datas[9] << 8) | datas[8]);    // GY
    raw_data[5] = (int16_t)((datas[11] << 8) | datas[10]);   // GZ
    return 0;
}

int QMI8658A::read_convert_print_data()
{
    int16_t raw_data[6] = {0};
    float convert_datas[6] = {0};

    if (read_raw_data(raw_data) != 0) {
        ESP_LOGE(TAG, "Failed to read raw data");
        return -1;
    }

    // 转换数据
    this->convert_data(raw_data, convert_datas, ACCRANGE, GYRRANGE);

    // 计算加速度模长
    float am = this->calculate_accele_magnitude(convert_datas);

    // 打印转换后的数据
    ESP_LOGI(TAG, "Converted data: AX: %f, AY: %f, AZ: %f, GX: %f, GY: %f, GZ: %f, AM: %f (dps)",
             convert_datas[0], convert_datas[1], convert_datas[2], convert_datas[3], convert_datas[4], convert_datas[5], am);

    return 0;
}

int QMI8658A::calibration_acc_gry(float *outdata)
{
    if (outdata == NULL) {
        ESP_LOGE(TAG, "Output data pointer is null");
        return -1;
    }

    int16_t raw_data[6] = {0};
    float convert_datas[6] = {0};
    float old_am = 0.0f, am = 0.0f;
    float (*DATA)[6] = (float(*)[6])pvPortMalloc(MIN_COLLECTION_COUNT * sizeof(float[6]));
    if (DATA == NULL) {
        ESP_LOGE(TAG, "Failed to allocate memory for DATA");
        return -1;
    }

    // 计数器
    int i = 0;
    while ( i < MIN_COLLECTION_COUNT) {
        if (0 != read_raw_data(raw_data)) {
            ESP_LOGE(TAG, "Failed to read raw data");
            vPortFree(DATA);
            return -1;
        }

        // 转换数据
        this->convert_data(raw_data, convert_datas, ACCRANGE, GYRRANGE);
        
        // 计算加速度模长
        am = this->calculate_accele_magnitude(convert_datas);

        // 判断当前加速度模长与上一次的插值是否小于阈值， 若小于则认为处于静止状态
        if (fabs(am - old_am) < STATIONARY_THRESHOLD) {
            // 计算陀螺仪平均值
            DATA[i][0] = convert_datas[0];
            DATA[i][1] = convert_datas[1];
            DATA[i][2] = convert_datas[2];
            DATA[i][3] = convert_datas[3];
            DATA[i][4] = convert_datas[4];
            DATA[i][5] = convert_datas[5];
            i++;
        } else {
            // 若不处于静止状态，更新oled_am为当前的加速度模长
            old_am = am;
        }
    }

    calculate_averages(DATA, outdata);

    // 释放内存
    vPortFree(DATA);

    // 打印校准结果，输出AX,AY,AZ,GX,GY,GZ轴的平均值
    ESP_LOGI(TAG, "Calibration result: AX: %f, AY: %f, AZ: %f, GX: %f, GY: %f, GZ: %f",
             outdata[0], outdata[1], outdata[2], outdata[3], outdata[4], outdata[5]);
    
    return 0;

}

int QMI8658A::cyr_cod(void)
{
    uint8_t data = 0x00;
    // 禁用传感器
    // 向寄存器CTRL7写入0x00，禁用加速度计和陀螺仪
    if (write_regs(CTRL7, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL7");
        return -1;
    }
    // 通CTRL9命令发出CTRL_CMD_ON_DEMAND_CALIBRATION(0xA2)命令
    data = 0xA2;
    if (write_regs(CTRL9, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL9");
        return -1;
    }

    // 等待1.5秒,让传感器完成CTRL命令
    vTaskDelay(pdMS_TO_TICKS(1500));

    // 等待STATUSINT第七位为1
    data = 0;
    while ((data & 0x80) != 0x80) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }

    // 向CTRL9寄存器写入CTRL_CMA_ACK(0X00)来确认
    data = 0x00;
    if (write_regs(CTRL9, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL9");
        return -1;
    }

    // 等待STATUSINT第七位为0
    data = 0xFF;
    while ((data & 0x80) == 0x80) {
        if (read_regs(STATUSINT, &data, 1) != 0) {
            ESP_LOGE(TAG, "Failed to read STATUSINT");
            return -1;
        }
    }

    // 读取COD_STATUS寄存器，检查校准是否成功
    data = 0;
    if (read_regs(COD_STATUS, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to read COD_STATUS");
        return -1;
    }
    // 若data不为0，则表示校准失败
    if (data) {
        ESP_LOGI(TAG, "Calibration failed");
        return -1;
    }

    // 开启加速度计和陀螺仪，同步采样模式
    // 向寄存器CTRL7写入0x83，启用加速度计和陀螺仪的同步采样模式
    data = 0x83;
    if (write_regs(CTRL7, &data, 1) != 0) {
        ESP_LOGE(TAG, "Failed to write CTRL7");
        return -1;
    }

    // 校准成功
    ESP_LOGI(TAG, "Calibration successful");
    return 0;

}

void QMI8658A::convert_data(int16_t *in_data, float *outdata, int accele_range, int gyro_range)
{
    float accele_factor = 0.0f, gyro_factor = 0.0f, compensate = 0.0f;

    // 计算加速度转换因子
    switch (accele_range) {
        case 2:
            accele_factor = 2.0f * 2 / 65536.0f;
            break;
        case 4:
            accele_factor = 4.0f * 2 / 65536.0f;
            break;
        case 8:
            accele_factor = 8.0f * 2 / 65536.0f;
            break;
        case 16:
            accele_factor = 16.0f * 2 / 65536.0f;
            break;
        default:
            accele_factor = 0.0f;
            break;
    }

    // 转换加速度计数据
    // 将原始加速度计数据乘以转换因子，得到以g位单位的加速度计数据
    for (int i = 0; i < 3; i++) {
        
        outdata[i] = in_data[i] * accele_factor;
    }

    // 计算陀螺仪转换因子
    switch (gyro_range) {
        case 16: // 16dps
            gyro_factor = 16.0f * 2 / 65536.0f;
            break;
        case 32: // 32dps
            gyro_factor = 32.0f * 2 / 65536.0f;
            break;
        case 64: // 64dps
            gyro_factor = 64.0f * 2 / 65536.0f;
            break;
        case 128: // 128dps
            gyro_factor = 128.0f * 2 / 65536.0f;
            break;
        case 256: // 256dps
            gyro_factor = 256.0f * 2 / 65536.0f;
            break;
        case 512: // 512dps
            gyro_factor = 512.0f * 2 / 65536.0f;
            break;
        case 1024: // 1024dps
            gyro_factor = 1024.0f * 2 / 65536.0f;
            break;
        case 2048: // 2048dps
            gyro_factor = 2048.0f * 2 / 65536.0f;
            break;  
        default:
            gyro_factor = 0.0f;
            break;
    }

    // 转换并不长陀螺仪数据
    // 将原始陀螺仪数据乘以转换因子，并减去对应的补偿值，得到以dps为单位的陀螺仪数据
    for (int i = 0; i < 3; i++) {
        switch (i) {
            case 3:
                compensate = this->gyro_compensate[3];
            break;
            case 4:
                compensate = this->gyro_compensate[4];
            break;
            case 5:
                compensate = this->gyro_compensate[5];
            break;
            default:
            break;
        }
        outdata[i] = in_data[i] * gyro_factor - compensate;
    }
}

float QMI8658A::calculate_accele_magnitude(float *outdata)
{
    // 计算加速度计的模长
    float magnitude = sqrt(outdata[0] * outdata[0] + outdata[1] * outdata[1] + outdata[2] * outdata[2]);
    return magnitude;
}

void QMI8658A::get_g_dps(float *outdata)
{
    int16_t raw_data[6] = {0};
    if (read_raw_data(raw_data) != 0) {
        ESP_LOGE(TAG, "Failed to read raw data");
        return;
    }

    // 将原始数据转换为实际的物理值
    convert_data(raw_data, outdata, ACCRANGE, GYRRANGE);
}

int QMI8658A::write_regs(uint8_t cmd, uint8_t *value, uint8_t count)
{
    // 开辟空间，用于存储写入的命令和数据
    uint8_t *data = (uint8_t *)pvPortMalloc(count + 1);
    if (data == NULL) {
        ESP_LOGE(TAG, "Failed to allocate memory for I2C write");
        return -1;
    }
    // 将命令和数据存储到数组中
    data[0] = cmd;
    memcpy(data + 1, value, count);
    int rst = 0;
    if (ESP_OK != i2c_master_transmit(this->m_i2c_dev, data, count + 1, 1000)) {
        ESP_LOGE(TAG, "I2C write failed");
        rst = -1;
    }
    vPortFree(data);
    return rst;
}

int QMI8658A::read_regs(uint8_t cmd, uint8_t *value, uint8_t count)
{
    if (ESP_OK != i2c_master_transmit_receive(this->m_i2c_dev, &cmd, 1, value, count, 1000)) {
        ESP_LOGE(TAG, "Failed to read register");
        return -1;
    }
    return 0;
}

void QMI8658A::calculate_averages(float data[MIN_COLLECTION_COUNT][6], float *outdata)
{
    float **sorted_data = (float **)pvPortMalloc(6 * sizeof(float *));
    if (sorted_data == NULL) {
        ESP_LOGE(TAG, "Failed to allocate memory for sorted data");
        return;
    }

    for (int i = 0; i < 6; i++) {
        sorted_data[i] = (float *)pvPortMalloc(MIN_COLLECTION_COUNT * sizeof(float));
        if (sorted_data[i] == NULL) {
            ESP_LOGE(TAG, "Failed to allocate memory for sorted data");
            for (int j = 0; j < i; j++) {
                vPortFree(sorted_data[j]);
            }
            vPortFree(sorted_data);
            return;
        }
    }

    // 将数据复制到sorted_data中
    for (int i = 0; i < MIN_COLLECTION_COUNT; i++) {
        for (int j = 0; j < 6; j++) {
            sorted_data[j][i] = data[i][j];
        }
    }

    // 对每一列进行排序
    for (int i = 0; i < 6; i++) {
        qsort(sorted_data[i], MIN_COLLECTION_COUNT, sizeof(float), QMI8658A::compare_float);
    }

    // 计算中间USED_DATA_COUNT 条数据的起始索引
    int start_index = (MIN_COLLECTION_COUNT - USED_DATA_COUNT) / 2;
    // 计算中间USED_DATA_COUNT 条数据的平均值
    for (int i = 0; i < 6; i++) {
        float sum = 0.0f;
        for (int j = start_index; j < start_index + USED_DATA_COUNT; j++) {
            sum += sorted_data[i][j];
        }
        outdata[i] = sum / USED_DATA_COUNT;
    }

    // 释放内存
    for (int i = 0; i < 6; i++) {
        vPortFree(sorted_data[i]);
    }
    vPortFree(sorted_data);
}
